#include "Handler.h"


extern CSettings g_setting;
extern CServoManager g_servos;
extern CKT404A g_kt404a;


CHandler::CHandler()
{
}

CHandler::~CHandler()
{
}

// 0xFB 0xBF len(1B) cmd(1B) params(N*B) Checksum(1B) 0xED
// Checksum = LOW_B(len+cmd+params)
uint8_t CHandler::Checksum(int size, const uint8_t *data)
{
	uint32_t sum = 0;
	for (int i = 2;i < size - 2;i++) {
		sum += data[i];
	}
	return (sum & 0xFF);
}

AeRet_t CHandler::HandleCmd(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	// Checking cmd...
	if (size < 6) {
		return AE_RET_ERROR;
	}
	if (data[0] != 0xFB || data[1] != 0xBF) {
		return AE_RET_UNSUPPORT;
	}
	uint8_t len = data[2];
	uint8_t sum = Checksum(size, data);
	if (len != size || data[size-2] != sum || data[size-1] != 0xED) {
		return AE_RET_ERROR;
	}

	// Process cmd...
	AeRet_t ret = AE_RET_SUCCESS;
	switch (data[3])
	{
	// Version
	case AE_PCMD_GET_VERSION:
		LOG("AE_PCMD_GET_VERSION");
		ret = getVersion(size, data, expect, out);
		break;
	// Storage
	case AE_PCMD_GET_STORAGE:
		LOG("AE_PCMD_GET_STORAGE");
		ret = getStorage(size, data, expect, out);
		break;
	// Action
	case AE_PCMD_GET_ACTIONS:
		LOG("AE_PCMD_GET_ACTIONS");
		ret = getActions(size, data, expect, out);
		break;
	case AE_PCMD_GETACT_SENDING:
		LOG("AE_PCMD_GETACT_SENDING");
		break;
	case AE_PCMD_GETACT_FINISHED:
		LOG("AE_PCMD_GETACT_FINISHED");
		break;
	case AE_PCMD_PLAY_ACTION:
		LOG("AE_PCMD_PLAY_ACTION");
		ret = playAction(size, data, expect, out);
		break;
	case AE_PCMD_PLAYACT_SENDING:
		LOG("AE_PCMD_PLAYACT_SENDING");
		break;
	case AE_PCMD_PLAYACT_FINISHED:
		LOG("AE_PCMD_PLAYACT_FINISHED");
		break;
	case AE_PCMD_STOP_ACTION:
		LOG("AE_PCMD_STOP_ACTION");
		ret = stopAction(size, data, expect, out);
		break;
	case AE_PCMD_CTRL_ACTION:
		LOG("AE_PCMD_CTRL_ACTION");
		ret = ctrlAction(size, data, expect, out);
		break;
	case AE_PCMD_DELETE_ACTION:
		LOG("AE_PCMD_DELETE_ACTION");
		ret = deleteAction(size, data, expect, out);
		break;
	case AE_PCMD_UPLOAD_ACTION:
		LOG("AE_PCMD_UPLOAD_ACTION");
		ret = uploadAction(size, data, expect, out);
		break;
	case AE_PCMD_UPLOADACT_SENDING:
		LOG("AE_PCMD_UPLOADACT_SENDING");
		ret = uploadActSending(size, data, expect, out);
		break;
	case AE_PCMD_UPLOADACT_FINISHED:
		LOG("AE_PCMD_UPLOADACT_FINISHED");
		ret = uploadActFinished(size, data, expect, out);
		break;
	// Voice
	case AE_PCMD_OFFON_VOICE:
		LOG("AE_PCMD_OFFON_VOICE");
		ret = offOnVoice(size, data, expect, out);
		break;
	case AE_PCMD_SET_VOLUME:
		LOG("AE_PCMD_SET_VOLUME");
		ret = setVolume(size, data, expect, out);
		break;
	case AE_PCMD_PLAY_MUSIC:
		LOG("AE_PCMD_PLAY_MUSIC");
		ret = playMusic(size, data, expect, out);
		break;
	case AE_PCMD_STOP_MUSIC:
		LOG("AE_PCMD_STOP_MUSIC");
		ret = stopMusic(size, data, expect, out);
		break;
	case AE_PCMD_CTRL_MUSIC:
		LOG("AE_PCMD_CTRL_MUSIC");
		ret = ctrlMusic(size, data, expect, out);
		break;
	// Servo
	case AE_PCMD_SET_SERVO_LED:
		LOG("AE_PCMD_SET_SERVO_LED");
		ret = setServoLed(size, data, expect, out);
		break;
	case AE_PCMD_SET_SERVOS_LED:
		LOG("AE_PCMD_SET_SERVOS_LED");
		ret = setServosLed(size, data, expect, out);
		break;
	case AE_PCMD_SET_SERVO_ANGLE:
		LOG("AE_PCMD_SET_SERVO_ANGLE");
		ret = setServoAngle(size, data, expect, out);
		break;
	case AE_PCMD_SET_SERVOS_ANGLE:
		LOG("AE_PCMD_SET_SERVOS_ANGLE");
		ret = setServosAngle(size, data, expect, out);
		break;
	case AE_PCMD_GET_SERVO_ANGLE:
		LOG("AE_PCMD_GET_SERVO_ANGLE");
		ret = getServoAngle(size, data, expect, out);
		break;
	case AE_PCMD_GET_SERVOS_ANGLE:
		LOG("AE_PCMD_GET_SERVOS_ANGLE");
		ret = getServosAngle(size, data, expect, out);
		break;
	case AE_PCMD_SET_SERVO_OFFSET:
		LOG("AE_PCMD_SET_SERVO_OFFSET");
		ret = setServoOffset(size, data, expect, out);
		break;
	case AE_PCMD_SET_SERVOS_OFFSET:
		LOG("AE_PCMD_SET_SERVOS_OFFSET");
		ret = setServosOffset(size, data, expect, out);
		break;
	case AE_PCMD_GET_SERVO_OFFSET:
		LOG("AE_PCMD_GET_SERVO_OFFSET");
		ret = getServoOffset(size, data, expect, out);
		break;
	case AE_PCMD_GET_SERVOS_OFFSET:
		LOG("AE_PCMD_GET_SERVOS_OFFSET");
		ret = getServosOffset(size, data, expect, out);
		break;
	case AE_PCMD_LOCK_SERVOS:
		LOG("AE_PCMD_LOCK_SERVOS");
		ret = lockServos(size, data, expect, out);
		break;
	case AE_PCMD_UNLOCK_SERVOS:
		LOG("AE_PCMD_UNLOCK_SERVOS");
		ret = unlockServos(size, data, expect, out);
		break;
	case AE_PCMD_GET_SERVO_FIRMWARE:
		LOG("AE_PCMD_GET_SERVO_FIRMWARE");
		ret = getServoFirmware(size, data, expect, out);
		break;
	case AE_PCMD_GET_SERVOS_FIRMWARE:
		LOG("AE_PCMD_GET_SERVOS_FIRMWARE");
		ret = getServosFirmware(size, data, expect, out);
		break;
  case AE_PCMD_START_TEACHING:
    LOG("AE_PCMD_START_TEACHING");
    ret = startTeaching(size, data, expect, out);
    break;
  case AE_PCMD_STOP_TEACHING:
  	LOG("AE_PCMD_STOP_TEACHING");
    ret = stopTeaching(size, data, expect, out);
    break;
  case AE_PCMD_TEACH_SENDING:
    LOG("AE_PCMD_TEACH_SENDING");
    break;
  case AE_PCMD_MOVING:
    LOG("AE_PCMD_MOVING");
    break;
	default:
		ret = AE_RET_UNSUPPORT;
		break;
	}

	return ret;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: version(4B) // 10bytes
AeRet_t CHandler::getVersion(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		expect = 10;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_VERSION;
		putU32(pos, out, g_setting.m_set.version);
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: total(4B) used(4B) // 14bytes
AeRet_t CHandler::getStorage(int size, const uint8_t *data, int &expect, uint8_t *out)
{
  AeRet_t ret = AE_RET_SUCCESS;

  do {
    uint32_t total = SPIFFS.totalBytes();
    uint32_t used = SPIFFS.usedBytes();
    
    expect = 14;
    int pos = 0;
    out[pos++] = 0xFB;
    out[pos++] = 0xBF;
    out[pos++] = expect;
    out[pos++] = AE_PCMD_GET_STORAGE;
		putU32(pos, out, total);
    putU32(pos, out, used);
    out[pos++] = Checksum(expect, out);
    out[pos++] = 0xED;
  } while(false);

  return ret;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CHandler::getActions(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_ACTIONS;
		out[pos++] = AE_RET_SUCCESS;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
		ret = AE_RET_GET_ACTIONS_TASK;
	} while(false);

	return ret;
}

// Pack: action(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CHandler::playAction(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		String action = "";
		for (int i = 4;i < size - 2;i++) {
			action += char(data[i]);
		}

		AeRet_t inRet = AE_RET_SUCCESS;
		do {
			if (!CActionOpt::Exists(action)) {
				inRet = AE_RET_NOT_EXISTS;
				break;
			}
			AeActHeader_t header;
			if (!CActionOpt::ReadHeader(action, header)) {
				inRet = AE_RET_ERROR;
				break;
			}

			// Play mp3 if not mute
			if (1 != g_setting.m_set.mute && 0 != header.mp3) {
				g_kt404a.SetVoice(g_setting.m_set.volume);
				g_kt404a.Play(header.mp3Dir, header.mp3Idx);
			}

			// Playing action
			g_setting.m_set.state = AE_STS_PLAYING;
			g_setting.m_set.action = action;
			g_setting.m_set.seqCnt = header.seqCnt;
			g_setting.m_set.nextSeq = 0;
			g_setting.m_set.servoMsec = 0;
			g_setting.m_set.waitMsec = 0;
			g_setting.m_set.startMsec = millis();
		} while(false);

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_PLAY_ACTION;
		out[pos++] = inRet;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::stopAction(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		// Stop mp3
		g_kt404a.Stop();

		// Stop action
    if (g_setting.m_set.state == AE_STS_PLAYING
      || g_setting.m_set.state == AE_STS_PAUSED) {
		  g_setting.m_set.state = AE_STS_IDEL;
	  	g_setting.m_set.action = "";
		  g_setting.m_set.seqCnt = 0;
		  g_setting.m_set.nextSeq = 0;
		  g_setting.m_set.servoMsec = 0;
		  g_setting.m_set.waitMsec = 0;
		  g_setting.m_set.startMsec = 0;
    }

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_STOP_ACTION;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: (0x00-pause/0x01-resume)(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::ctrlAction(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		if (data[4] == 0x00 && g_setting.m_set.state == AE_STS_PLAYING) {
			// Pause
			g_setting.m_set.state = AE_STS_PAUSED;
			g_kt404a.Pause();
		}
		else if (data[4] == 0x01 && g_setting.m_set.state == AE_STS_PAUSED) {
			// Resume
			g_setting.m_set.state = AE_STS_PLAYING;
			g_kt404a.Resume();
		}

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_CTRL_ACTION;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: action(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Unpack: ret(1B) action(AE_PATH_MAX) // 7+AE_PATH_MAX bytes
AeRet_t CHandler::deleteAction(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		String action = "";
		for (int i = 4;i < (size - 2);i++) {
			action += char(data[i]);
		}
		if (CActionOpt::Exists(action)) {
			SPIFFS.remove(action);
		}

		expect = 7 + AE_PATH_MAX;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_DELETE_ACTION;
		out[pos++] = 0x00;
		memset(out + pos, 0x00, AE_PATH_MAX);
		memcpy(out + pos, action.c_str(), action.length());
		pos += AE_PATH_MAX;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: action(AE_PATH_MAX) AeActHeader_t // 6+AE_PATH_MAX+sizeof(AeActHeader_t) bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CHandler::uploadAction(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		if (size < 6 + AE_PATH_MAX + sizeof(AeActHeader_t)) {
			ret = AE_RET_ERROR;
			break;
		}

		AeRet_t inRet = AE_RET_SUCCESS;
		String action = "";
		for (int i = 4;i < (size - 2 - sizeof(AeActHeader_t));i++) {
			action += char(data[i]);
		}
		AeActHeader_t header;
		memcpy(&header, data + (size - 2 - sizeof(AeActHeader_t)), sizeof(AeActHeader_t));
		if (!CActionOpt::WriteHeader(action, header)) {
			inRet = AE_RET_ERROR;
		}

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_UPLOAD_ACTION;
		out[pos++] = inRet;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: action(AE_PATH_MAX) AeActPose_t // 6+AE_PATH_MAX+sizeof(AeActPose_t) bytes
// Unpack: ret(1B) seq(4B) // 11bytes
AeRet_t CHandler::uploadActSending(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		if (size < 6 + AE_PATH_MAX + sizeof(AeActPose_t)) {
			ret = AE_RET_ERROR;
			break;
		}

		AeRet_t inRet = AE_RET_SUCCESS;
		String action = "";
		for (int i = 4;i < (size - 2 - sizeof(AeActPose_t));i++) {
			action += char(data[i]);
		}
		AeActPose_t pose;
		memcpy(&pose, data + (size - 2 - sizeof(AeActPose_t)), sizeof(AeActPose_t));
		if (!CActionOpt::WritePose(action, pose.seq, pose)) {
			inRet = AE_RET_ERROR;
		}

		expect = 11;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_UPLOADACT_SENDING;
		out[pos++] = inRet;
		putU32(pos, out, pose.seq);
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Req: action(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Ret: ret(1B) // 7bytes
AeRet_t CHandler::uploadActFinished(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		if (size < 6 + AE_PATH_MAX) {
			ret = AE_RET_ERROR;
			break;
		}

		AeRet_t inRet = AE_RET_SUCCESS;
		String action = "";
		for (int i = 4;i < (size - 2);i++) {
			action += char(data[i]);
		}
		if (!CActionOpt::Exists(action)) {
			inRet = AE_RET_ERROR;
		}

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_UPLOADACT_FINISHED;
		out[pos++] = inRet;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: (0x00-mute/0x01-normal)(1B) // 7bytes
// Unpack: 0x00 // 7bytes
AeRet_t CHandler::offOnVoice(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		if (data[4] == 0x00) {
			// Mute
			g_setting.m_set.mute = 0x01;
			g_setting.Save();
			g_kt404a.SetVoice(0);
		}
		else {
			// Not mute
			g_setting.m_set.mute = 0x00;
			g_setting.Save();
			g_kt404a.SetVoice(g_setting.m_set.volume);
		}

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_OFFON_VOICE;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: volume(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::setVolume(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint8_t volume = data[4];
		g_setting.m_set.volume = volume;
		g_setting.Save();
		g_kt404a.SetVoice(volume);

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_SET_VOLUME;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: dir(1B) idx(1B) // 8bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::playMusic(int size, const uint8_t *data, int &expect, uint8_t *out)
{
  AeRet_t ret = AE_RET_SUCCESS;

  do {
    uint8_t dir = data[4];
    uint8_t idx = data[5];
    g_kt404a.SetVoice(g_setting.m_set.volume);
    g_kt404a.Play(dir, idx);

    expect = 7;
    int pos = 0;
    out[pos++] = 0xFB;
    out[pos++] = 0xBF;
    out[pos++] = expect;
    out[pos++] = AE_PCMD_PLAY_MUSIC;
    out[pos++] = 0x00;
    out[pos++] = Checksum(expect, out);
    out[pos++] = 0xED;
  } while(false);

  return ret;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::stopMusic(int size, const uint8_t *data, int &expect, uint8_t *out)
{
  AeRet_t ret = AE_RET_SUCCESS;

  do {
    g_kt404a.Stop();

    expect = 7;
    int pos = 0;
    out[pos++] = 0xFB;
    out[pos++] = 0xBF;
    out[pos++] = expect;
    out[pos++] = AE_PCMD_STOP_MUSIC;
    out[pos++] = 0x00;
    out[pos++] = Checksum(expect, out);
    out[pos++] = 0xED;
  } while(false);

  return ret;
}

// Pack: (0x00-pause/0x01-resume)(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::ctrlMusic(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

  do {
    if (0x00 == data[4]) {
			g_kt404a.Pause();
		}
		else {
			g_kt404a.Resume();
		}

    expect = 7;
    int pos = 0;
    out[pos++] = 0xFB;
    out[pos++] = 0xBF;
    out[pos++] = expect;
    out[pos++] = AE_PCMD_CTRL_MUSIC;
    out[pos++] = 0x00;
    out[pos++] = Checksum(expect, out);
    out[pos++] = 0xED;
  } while(false);

  return ret;
}

// Pack: idx(1B) (0x00-off/0x01-on)(1B) // 8bytes
// Unpack: idx(1B) 0x00(1B) // 8bytes
AeRet_t CHandler::setServoLed(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint8_t idx = data[4];
		bool on = (0x00 != data[5]);
		g_servos.SetLed(idx-1, on);

		expect = 8;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_SET_SERVO_LED;
		out[pos++] = idx;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: (0x00-off/0x01-on)(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::setServosLed(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		bool on = (0x00 != data[4]);
		for (int i = 0;i < SERVO_MAX_CNT;i++) {
			g_servos.SetLed(i, on);
		}

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_SET_SERVOS_LED;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: idx(1B) Angle(2B) Time(1B) LckTime(1B) // 11bytes
// Unpack: idx(1B) ret(1B) // 8bytes
// Time unit is in 20ms!!!
AeRet_t CHandler::setServoAngle(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint8_t idx = data[4];
		uint16_t angle = (data[5] << 8) | data[6];
		uint32_t time = data[7] * 20;
		uint32_t lcktime = data[8] * 20;
		
		// Time-consuming operation to task...
		// Always return success!!!
		g_setting.m_set.idx = idx;
		g_setting.m_set.angle = angle;
		g_setting.m_set.timeMsec = time;
		g_setting.m_set.lckTimeMsec = lcktime;

		expect = 8;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_SET_SERVO_ANGLE;
		out[pos++] = idx;
		out[pos++] = AE_RET_SUCCESS;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
		ret = AE_RET_SET_ANGLE_TASK;
	} while(false);

	return ret;
}

// Pack: angles(AE_SERVO_MAX*2B) time(1B) lcktime(1B) // 8+2*AE_SERVO_MAX bytes
// Unpack: (0x00-success/ret)(AE_SERVO_MAX*1B) // 6+AE_SERVO_MAX bytes
// Time unit is in 20ms!!!
AeRet_t CHandler::setServosAngle(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint16_t angles[AE_SERVO_MAX];
		for (int i = 0;i < AE_SERVO_MAX;i++) {
			angles[i] = (data[4 + i * 2] << 8) | data[4 + i * 2 + 1];
		}
		uint32_t time = data[size - 4] * 20;
		uint32_t lcktime = data[size - 3] * 20;
		AeRet_t rets[AE_SERVO_MAX];
		memset(rets, 0x00, sizeof(rets));
		
		// Time-consuming operation to task...
		// Always return success
		for (int i = 0;i < AE_SERVO_MAX;i++) {
			g_setting.m_set.angles[i] = angles[i];
		}
		g_setting.m_set.timeMsec = time;
		g_setting.m_set.lckTimeMsec = lcktime;

		expect = 6 + AE_SERVO_MAX;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_SET_SERVOS_ANGLE;
		for (int i = 0;i < AE_SERVO_MAX;i++) {
			out[pos++] = rets[i];
		}
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
		ret = AE_RET_SET_ANGLES_TASK;
	} while(false);

	return ret;
}

// Pack: idx(1B) // 7bytes
// Unpack: idx(1B) Angle(2B,0xFF00-Err) // 9bytes
AeRet_t CHandler::getServoAngle(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint8_t idx = data[4];
		uint16_t angle = 0;
		if (SERVO_RET_SUCCESS != g_servos.GetAngle(idx-1, angle)) {
			angle = 0xFF00;
		}

		expect = 9;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_SERVO_ANGLE;
		out[pos++] = idx;
		out[pos++] = (angle >> 8) & 0xFF;
		out[pos++] = angle & 0xFF;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00 // 7bytes
// Unpack: Angles(AE_SERVO_MAX*2B,0xFF00-Err) // 6+2*AE_SERVO_MAX bytes
AeRet_t CHandler::getServosAngle(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint16_t angles[AE_SERVO_MAX];
		memset(angles, 0x00, sizeof(angles));
		for (int i = 0;i < SERVO_MAX_CNT;i++) {
			if (SERVO_RET_SUCCESS != g_servos.GetAngle(i, angles[i])) {
				angles[i] = 0xFF00;
			}
		}

		expect = 6 + 2 * AE_SERVO_MAX;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_SERVOS_ANGLE;
		for (int i = 0;i < AE_SERVO_MAX;i++) {
			out[pos++] = (angles[i] >> 8) & 0xFF;
			out[pos++] = angles[i] & 0xFF;
		}
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: idx(1B) offset(2B:signed) // 9bytes
// Unpack: idx(1B) ret(1B) // 8bytes
AeRet_t CHandler::setServoOffset(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		AeRet_t inRet = AE_RET_SUCCESS;
		uint8_t idx = data[4];
		int16_t offset = (data[5] << 8) | data[6];
		if (SERVO_RET_SUCCESS != g_servos.SetOffset(idx-1, offset)) {
			inRet = AE_RET_ERROR;
		}

		expect = 8;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_SET_SERVO_OFFSET;
		out[pos++] = idx;
		out[pos++] = inRet;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: offsets(AE_SERVO_MAX*2B:signed) // 6+2*AE_SERVO_MAX bytes
// Unpack: (0x00-success/ret)(AE_SERVO_MAX*1B) // 6+AE_SERVO_MAX bytes
AeRet_t CHandler::setServosOffset(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		if (size < 6 + AE_SERVO_MAX * 2) {
			ret = AE_RET_ERROR;
			break;
		}

		int16_t offsets[AE_SERVO_MAX];
		for (int i = 0;i < AE_SERVO_MAX;i++) {
			offsets[i] = (data[4 + i * 2] << 8) | (data[4 + i * 2 + 1]);
		}
		AeRet_t rets[AE_SERVO_MAX];
		memset(rets, 0x00, sizeof(rets));
		for (int i = 0;i < SERVO_MAX_CNT;i++) {
			if (SERVO_RET_SUCCESS != g_servos.SetOffset(i, offsets[i])) {
				rets[i] = AE_RET_ERROR;
			}
		}

		expect = 6 + AE_SERVO_MAX;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_SET_SERVOS_OFFSET;
		for (int i = 0;i < AE_SERVO_MAX;i++) {
			out[pos++] = rets[i];
		}
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: idx(1B) // 7bytes
// Unpack: idx(1B) offset(2B:0x88xx-err) // 9bytes
AeRet_t CHandler::getServoOffset(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint8_t idx = data[4];
		int16_t offset = 0;
		if (SERVO_RET_SUCCESS != g_servos.GetOffset(idx-1, offset)) {
			offset = 0x8888;
		}

		expect = 9;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_SERVO_OFFSET;
		out[pos++] = idx;
		out[pos++] = (offset >> 8) & 0xFF;
		out[pos++] = offset & 0xFF;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00 // 7bytes
// Unpack: offsets(0x88xx-err)(AE_SERVO_MAX*2B) // 6+2*AE_SERVO_MAX bytes
AeRet_t CHandler::getServosOffset(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		int16_t offsets[AE_SERVO_MAX];
		memset(offsets, 0x00, sizeof(offsets));
		for (int i = 0;i < SERVO_MAX_CNT;i++) {
			if (SERVO_RET_SUCCESS != g_servos.GetOffset(i, offsets[i])) {
				offsets[i] = 0x8888;
			}
		}

		expect = 6 + 2 * AE_SERVO_MAX;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_SERVOS_OFFSET;
		for (int i = 0;i < AE_SERVO_MAX;i++) {
			out[pos++] = (offsets[i] >> 8) & 0xFF;
			out[pos++] = offsets[i] & 0xFF;
		}
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::lockServos(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		for (int i = 0;i < SERVO_MAX_CNT;i++) {
			g_servos.Lock(i);
		}

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_LOCK_SERVOS;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CHandler::unlockServos(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		for (int i = 0;i < SERVO_MAX_CNT;i++) {
			g_servos.Unlock(i);
		}

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_UNLOCK_SERVOS;
		out[pos++] = 0x00;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: idx(1B) // 7bytes
// Unpack: idx(1B) firmware(4B) // 11bytes
AeRet_t CHandler::getServoFirmware(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint8_t idx = data[4];
		uint32_t firmware = 0;
		g_servos.GetFirmware(idx-1, firmware);

		expect = 11;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_SERVO_FIRMWARE;
		out[pos++] = idx;
		putU32(pos, out, firmware);
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00 // 7bytes
// Unpack: firmwares(AE_SERVO_MAX*4B) // 6+4*AE_SERVO_MAX bytes
AeRet_t CHandler::getServosFirmware(int size, const uint8_t *data, int &expect, uint8_t *out)
{
	AeRet_t ret = AE_RET_SUCCESS;

	do {
		uint32_t firmwares[AE_SERVO_MAX];
		memset(firmwares, 0x00, sizeof(firmwares));
		for (int i = 0;i < SERVO_MAX_CNT;i++) {
			g_servos.GetFirmware(i, firmwares[i]);
		}

		expect = 6 + 4 * AE_SERVO_MAX;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_GET_SERVOS_FIRMWARE;
		for (int i = 0; i < AE_SERVO_MAX;i++) {
			putU32(pos, out, firmwares[i]);
		}
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: name(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Unpack: waitMs(4B) // 10bytes
AeRet_t CHandler::startTeaching(int size, const uint8_t *data, int &expect, uint8_t *out)
{
  AeRet_t ret = AE_RET_SUCCESS;

	do {
		if (size < 6 + AE_PATH_MAX) {
			ret = AE_RET_ERROR;
			break;
		}

		String action = "";
		for (int i = 4;i < (size - 2);i++) {
			action += char(data[i]);
		}
    uint32_t waitMs = 0;
    if (g_setting.m_set.state != AE_STS_IDEL) {
      waitMs = 0;
    }
    else {
      // Write action header
      AeActHeader_t header;
      header.servoCnt = g_setting.m_set.servoCnt;
	    header.lenMsec = 0;
	    header.seqCnt = 0;
	    header.mp3 = 0;
	    header.mp3Dir = 0;
	    header.mp3Idx = 0;
      if (!CActionOpt::WriteHeader(action, header)) {
        waitMs = 0;
        SPIFFS.remove(action);
      }
      else {
        for (int i = 0;i < AE_SERVO_MAX;i++) {
          g_setting.m_set.lastAngles[i] = g_servos.GetInitAngle(i);
        }
        waitMs = 20; // Sampling interval
        g_setting.m_set.state = AE_STS_TEACHING;
        g_setting.m_set.action = action;
        g_setting.m_set.seqCnt = 0;
        g_setting.m_set.servoMsec = waitMs; 
	      g_setting.m_set.waitMsec = waitMs;
        g_setting.m_set.startMsec = millis() + 2000; // Delay start for 2 secs
      }
    }

		expect = 10;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_START_TEACHING;
    putU32(pos, out, waitMs);
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

// Pack: 0x00 // 7bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CHandler::stopTeaching(int size, const uint8_t *data, int &expect, uint8_t *out)
{
  AeRet_t ret = AE_RET_SUCCESS;

	do {
    AeRet_t inRet = AE_RET_SUCCESS;
    if (g_setting.m_set.state == AE_STS_TEACHING) {
      // Update action header
      AeActHeader_t header;
      header.servoCnt = g_setting.m_set.servoCnt;
	    header.lenMsec = g_setting.m_set.seqCnt * g_setting.m_set.waitMsec;
	    header.seqCnt = g_setting.m_set.seqCnt;
	    header.mp3 = 0;
	    header.mp3Dir = 0;
	    header.mp3Idx = 0;
      if (!CActionOpt::UpdateHeader(g_setting.m_set.action, header)) {
        inRet = AE_RET_ERROR;
        SPIFFS.remove(g_setting.m_set.action);
      }

      g_setting.m_set.state = AE_STS_IDEL;
      g_setting.m_set.action = "";
      g_setting.m_set.seqCnt = 0;
      g_setting.m_set.servoMsec = 0; 
	    g_setting.m_set.waitMsec = 0; 
      g_setting.m_set.startMsec = 0;
    }

		expect = 7;
		int pos = 0;
		out[pos++] = 0xFB;
		out[pos++] = 0xBF;
		out[pos++] = expect;
		out[pos++] = AE_PCMD_STOP_TEACHING;
		out[pos++] = inRet;
		out[pos++] = Checksum(expect, out);
		out[pos++] = 0xED;
	} while(false);

	return ret;
}

void CHandler::putU32(int &offset, uint8_t *data, uint32_t val)
{
	data[offset++] = (val >> 24) & 0xFF;
	data[offset++] = (val >> 16) & 0xFF;
	data[offset++] = (val >> 8) & 0xFF;
	data[offset++] = val & 0xFF;
}
